%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   José Vilca, Lounis Adouane and Youcef Mezouar                        %%
%   Institut Pascal                                                      %%
%                                                                        %%                                                         %%                                                                                             %%
%  Hybrid system for vehicle (Tricycle robot) navigation                                  %%
%  Version : April 2014                                                  %%
%  Dernière mise à jour : novembre 2014                                  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc
close all
clear all

%% Data of the VIPALAB (Characteristcs)
global vmax accmax Tsrob lrobot lbase wtrack rwheel rsteerangle
vipalabData     %Load the structure with the vipalab data
lrobot = vipalab.physical.lrobot;
lbase  = vipalab.physical.lbase;
wtrack = vipalab.physical.wtrack;
rwheel = vipalab.physical.rwheel;
vmax = vipalab.motor.vmax;
rsteerangle = vipalab.motor.rsteerangle*1.5;
vmaxsteer = 2*rsteerangle;
accmax = vipalab.motor.accmax;
Tsrob  = vipalab.Tsrob;

% Chasis parameters
paramChasisRob.a = lrobot;
paramChasisRob.b = vipalab.physical.wrobot;

%% Data from user to robot
% Initial position
rangrobot = 0*pi/180;                   % Initial orientation of the robot (radians)
vrob = 0;                               % Initial velocity of the robot (m/s)
ifrontwh = 10*pi/180;                   % Initial orientation of the front wheel of the robot (gamma)
iposrob = [-10;-2;rangrobot;ifrontwh];    % Initial position and orientation of the robot (x,y,theta,gamma)

% Final position
Xf = 16;  %6
Yf = 2.2; %0.5;        
fposrob = [Xf;Yf;rangrobot;0];          % Final position and orientation of the robot (x,y,theta,gamma)

[Thetadi,distf,EthRTf] = InitialParam(iposrob,fposrob);

erthreshd = lrobot*0.5;                 % Threshold of the error position of the robot to goal
erpos = norm(iposrob(1:2)-fposrob(1:2));% Compute of the erros distance between the robot and the target

%% Plot the environment
ifig = 1;
fig1 = figure(ifig);
plot(Xf,Yf,'xr','linewidth',2)
hold on;
axis([-10 20 -10 10]);
axis equal
grid on;
xlabel('x [m]','FontSize',16,'FontName','Times')
ylabel('y [m]','FontSize',16,'FontName','Times')

%% Plot obstacles
nObs = 4;
Obstacles = [];
aObst = [1.0 1.0 1.0 1.0];
bObst = [0.5 0.5 0.5 0.5];
OmegaObst = [0 45 0 0]*pi/180;
CenterObst = [-4 -2 8 12;-4 0.12 2 -4];% [x;y]

for i=1:nObs
paramObst(i).a = aObst(i);
paramObst(i).b = bObst(i);
paramObst(i).Omega = OmegaObst(i);
paramObst(i).Center = CenterObst(:,i);
paramObst(i).index = i;
auxObstacles = plotellipsegral(paramObst(i));
figobst = plot(auxObstacles(1,:), auxObstacles(2,:), ':k','linewidth',3); 

Obstacles = [Obstacles;auxObstacles];
end
paramEllip = [];

%% Initial data to Controller
global IndiceObstaclePlusProche epsatrac epsrepul alc blc Mu Rhor xTargetLC yTargetLC
IndiceObstaclePlusProche = 0;
eps = 0.75*lrobot;
epsatrac = 0.1*eps;
epsrepul = 0.2*eps;
RayonCycleLimite = 0;
Rhor = lrobot;
Mu = 0.2;
xTargetLC = 1500;
yTargetLC = 1500;

%% Initial variables
global erroposrob newtarget
newtarget = 0;

% Range sensor
global drang Tlidar
LxyDatNoise = [];
nLxyDat = 0;

% Init variables of the ellipse
paramObstHeu.a = 0;
paramObstHeu.b = 0;
paramObstHeu.Omega = 0;
paramObstHeu.Center = [0;0];

% Initialization of the plots
figCycleLim = [];
figCycleLimOA = [];  
figlineTarget = [];

%% Movement of the robot    
k = 0;
kf = 40/Tsrob; % Final time 40 s

% Data to save
posrob = [];
velrob = [];
Lyapval = [];
erroposrob = [];

%% Java connection
socketLMS = SocketClientForLMS(2003,'localhost');
socket = SocketClientForSMA(2002,'localhost');


while(k < kf)
    k = k + 1;
    %% Plot
    if k~=1
        delete(figbeamLas)
    end
    if k>2 && (rem(k,200)~=0)        
        delete(figrob,figChasisrob)
    end        
    
    % Plot Robot     
    [figrob,figChasisrob] = plotTricycleRecEllip(iposrob,paramChasisRob);    
                
    % Plot position of Laser sensor LIDAR SICK (range 4 m)
    [figLaserpos,figbeamLas,posobst,rangeLas,angleLas,xyDatNoise,nxyDat] = plotLIDARNoise(iposrob,Obstacles,nObs);
    
    %socketLMS.sendLMS(180,1,rangeLas,angleLas);
    %% Init modif: Add your code to send LIDAR data  
    % rangeLas: detection range in meters
    % angleLas: detection angle in radians (multiply by 180/pi to obtain
    % degree)
    
    socketLMS.sendLMS(180,1,rangeLas,angleLas);
    %% Fin modif
    
    %% Enclosing ellipse methods    
    if (nxyDat >= 3) || (nLxyDat >= 3)                                 
        % Select the ellipse to the controller
        paramEllip = paramObst;
        for i=1:nObs
            Reldist = norm(paramObst(i).Center - iposrob(1:2))-Tlidar(2); 
            if Reldist < drang
                paramEllip(i).a = paramEllip(i).a + eps;        
                paramEllip(i).b = paramEllip(i).b + eps;  
                eCycle = plotellipsegral(paramEllip(i));
                xTargetLC = 1500;
                yTargetLC = 1500;
%                 delete(figCycleLim)
                figCycleLim = plot(eCycle(1,:),eCycle(2,:),'--r'); 
            end        
        end     
    else
        paramEllip = [];        
    end
    pause(0.001)            
    LxyDatNoise = [LxyDatNoise xyDatNoise];
    nLxyDat = nLxyDat + nxyDat;
        
    %% Mobile robot
    % Condition to reach the static target    
    erpos = sqrt((Xf-iposrob(1))^2 + (Yf-iposrob(2))^2);      % Compute of the erros distance between robot and target    
    if erpos < erthreshd
        k = k - 1;
        break;
    end
    
    % Controller    
    % IndCA = 0 => Attraction to the target
    % IndCA = 1 => Obstacle avoidance
    % Target orientation is equal to the angle between the vehicle and target positions
    fposrob(3) = atan2( (fposrob(2)-iposrob(2)),(fposrob(1)-iposrob(1)) );
    % Commande = [vel_d, gamma_d, VFLyap1, VFLyap2, VFLyap3];
    [CommandeTarget CommandeOA IndicateurCA] = CoordinationControleursN(iposrob,fposrob,paramEllip,[Thetadi distf EthRTf]);                
    
    %Plot Cycle limit              
    if IndicateurCA    % Cycle limit                    
        paramEllipInf = paramEllip(IndiceObstaclePlusProche);
        paramEllipInf.a = alc;        
        paramEllipInf.b = blc;  
        eCycle = plotellipsegral(paramEllipInf);
        delete(figCycleLimOA,figlineTarget)  
        figCycleLimOA = plot(eCycle(1,:),eCycle(2,:),'--k');
        figlineTarget = plot([iposrob(1) Xf],[iposrob(2) Yf],'--c');
    end
          
    %% Init modif: Add your code to send and receive data from java            
    % Velocity and steering angle commands
    vel1 = CommandeTarget(1)*100; % scaled by 100
    angle1 = CommandeTarget(2)*180/pi*100; % scaled by 100
    vel2 = CommandeOA(1)*100; % scaled by 100
    angle2 =CommandeOA(2)*180/pi*100; % scaled by 100
    
    Commande = zeros(1,5);    
    if IndicateurCA
        Commande(3:5) = CommandeOA(3:5);  % Lyapunov values
        % cmd = [angle,vel]
        cmd = socket.updateD(vel1,angle1,vel2,angle2);
    else
        Commande(3:5) = CommandeTarget(3:5); % Lyapunov values
        % cmd = [angle,vel]
        cmd = socket.updateD(vel1,angle1,vel2,angle2);
    end
        
    velcmd = str2double(cmd{2});
    anglecmd = str2double(cmd{1});
    Commande(1) = velcmd/100;
    Commande(2) = (anglecmd*pi/180)/100;
    
    %% Fin modif    
    
    % Save position and Lyapunov function in a vector
    posrob = [posrob iposrob];
    velrob = [velrob Commande(1:2)'];
    Lyapval = [Lyapval sum(Commande(3:5))];
    
    % Kinematic model of the vehicle           
    vrob = Commande(1);
    ifrontwh = Commande(2);
    iposrob = TricycleKinmod(iposrob,vrob,ifrontwh);
          
end
trajrob = plot(posrob(1,:),posrob(2,:),'g','linewidth',3);
lh1 = legend([figrob(1) figobst trajrob],'Robot','Elliptic Obstacle','Robot Trajectory with proposed control law');
set(lh1,'FontSize',14,'FontName','Times New Roman');

%% Performance plots
figure(2) % Lyapunov function
plot((0:(k-1))*Tsrob,Lyapval,'g','linewidth',3)    
grid on;
xlabel(' t [s]','FontSize',16,'FontName','Times')
ylabel('Lyapunov function','FontSize',16,'FontName','Times')
lh2 = legend('Proposed control law');
set(lh2,'FontSize',14,'FontName','Times New Roman');

figure(3) % System errors
subplot(3,1,1)
plot((0:(k-1))*Tsrob,erroposrob(1,:),'g','linewidth',3)    
grid on;
ylabel('e_{x} [m]','FontSize',16,'FontName','Times')
subplot(3,1,2)
plot((0:(k-1))*Tsrob,erroposrob(2,:),'g','linewidth',3)    
grid on;
ylabel('e_{y} [m]','FontSize',16,'FontName','Times')
subplot(3,1,3)
plot((0:(k-1))*Tsrob,180/pi*erroposrob(3,:),'g','linewidth',3)    
grid on;
ylabel('e_{\theta} [degree]','FontSize',16,'FontName','Times')
xlabel(' t [s]','FontSize',16,'FontName','Times')
lh3 = legend('Proposed control law');
set(lh3,'FontSize',14,'FontName','Times New Roman');

figure(4) % Velocity commands
subplot(2,1,1)
plot((0:(k-1))*Tsrob,velrob(1,:),'g','linewidth',3)    
grid on;
ylabel('v [m/s]','FontSize',16,'FontName','Times')
subplot(2,1,2)
plot((0:(k-1))*Tsrob,velrob(2,:)*180/pi,'g','linewidth',3)    
grid on;
ylabel('\gamma_d [degree]','FontSize',16,'FontName','Times')
xlabel(' t [s]','FontSize',16,'FontName','Times')
lh4 = legend('Proposed control law');
set(lh4,'FontSize',14,'FontName','Times New Roman');